﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;
using UC_Framework;

namespace SwarmPlay
{
    static class SwarmMover
    {
        const float kDizzySpeedRatio = 0.8f;
        const float kSearchingSpeedRatio = 1.2f;

        public static Vector2 GetShortestVector(Vector2 pos, Rectangle rect, out bool isInside)
        {
            bool inXRange = pos.X > rect.Left && pos.X < rect.Right;
            bool inYRange = pos.Y > rect.Top && pos.Y < rect.Bottom;

            float x1 = rect.Left - pos.X;
            float x2 = rect.Right - pos.X;
            float y1 = rect.Top - pos.Y;
            float y2 = rect.Bottom - pos.Y;

            isInside = inXRange && inYRange;

            if (inXRange && inYRange)
            {
                float x = Math.Abs(x1) < Math.Abs(x2) ? x1 : x2;
                float y = Math.Abs(y1) < Math.Abs(y2) ? y1 : y2;
                if (Math.Abs(x) < Math.Abs(y))
                    return new Vector2(x, 0);
                else
                    return new Vector2(0, y);
            }
            else
            {
                float x = (pos.X <= rect.Left) ? rect.Left - pos.X : rect.Right - pos.X;
                float y = (pos.Y <= rect.Top) ? rect.Top - pos.Y : rect.Bottom - pos.Y;

                if (inXRange)
                    return new Vector2(0, y);

                if (inYRange)
                    return new Vector2(x, 0);

                return new Vector2(x, y);
            }
        }

        public static bool HitObstacles(this GameLevel_1 game, Sprite2 s, 
            out Vector2 shortestP2C, out bool isShortestInside)
        {
            Vector2 pos = s.getPos();

            float lenShortestP2C = float.PositiveInfinity;
            bool got = false;

            isShortestInside = false;
            shortestP2C = default(Vector2);

            foreach (Rectangle obstacle in game.obstacles)
            {
                bool isInside;
                Vector2 pathToClash = GetShortestVector(pos, obstacle, out isInside);

                if (pathToClash.Length() < lenShortestP2C)
                {
                    shortestP2C = pathToClash;
                    isShortestInside = isInside;
                    lenShortestP2C = pathToClash.Length();
                    got = true;
                }
            }

            return got;
        }


        public static float AvoidObstacles(this GameLevel_1 game, Sprite2 s,
            float dizzySpeedRatio, float searchingSpeedRatio)
        {
            Vector2 shortestP2C;
            bool isShortestInside;
            bool got = HitObstacles(game, s, out shortestP2C, out isShortestInside);

            if (!got)
                return 0f;
                
            float angle = s.getMoveAngle();

            Vector2 pos = s.getPos();
            if (isShortestInside)
            {
                s.setPos(pos + shortestP2C * 15f);
            }

            float dist = shortestP2C.Length();
            float speed = s.getMoveSpeed();
            Vector2 motionVec = new Vector2((float)Math.Cos(angle), (float)Math.Sin(angle));
            motionVec = motionVec * speed;

            Vector2 p2cNormed = shortestP2C / shortestP2C.Length();

            float speedToClash = p2cNormed.X * motionVec.X + p2cNormed.Y * motionVec.Y;
            Vector2 vecToClash = p2cNormed * speedToClash;
            Vector2 vecAlong = motionVec - vecToClash;

            float hitr = 0;
            if (isShortestInside)
            {
                Vector2 newMotionVec = vecAlong;

                if (newMotionVec.Length() == 0)
                {
                    angle = new Random().Next(2) > 0 ? angle + (float)Math.PI * 0.5f
                        : angle - (float)Math.PI * 0.5f;
                    if (angle < 0) angle += (float)Math.PI * 2;
                    if (angle > Math.PI * 2) angle -= (float)Math.PI * 2;
                }
                else
                {
                    angle = (float)(Math.Atan2(newMotionVec.Y, newMotionVec.X));
                }

                s.setMoveSpeed(game.swarmSpeed * dizzySpeedRatio);
                hitr = 1;
            }
            else if (speedToClash > 0)
            {
                float timeToClash = dist / speedToClash;
                if (timeToClash < 30)
                {
                    hitr = 1 - timeToClash / 30;
                    Vector2 newVecToClash = vecToClash * timeToClash / 30;
                    Vector2 newMotionVec = vecAlong + newVecToClash;
                        
                    System.Diagnostics.Trace.Assert(newMotionVec.Length() > 0);
                    angle = (float)(Math.Atan2(newMotionVec.Y, newMotionVec.X));
                    s.setMoveSpeed(game.swarmSpeed * searchingSpeedRatio);
                }
                else
                {
                    hitr = 0;
                }

                s.setMoveAngle(angle);
                s.setDisplayAngle(angle);
                
            }
            return hitr;
        }

        public static void AvoidObstacles(this GameLevel_1 game, Sprite2 s)
        {
            AvoidObstacles(game, s, kDizzySpeedRatio, kSearchingSpeedRatio);
        }
    }
}
